'''
Created on Nov 30, 2009

@author: Peter
'''
from numpy import array, eye, linalg
from math import cos, sin, sqrt
import math
def getZRotationMatrix(angleZ):
    """Returns a rotation matrix around the Z-Axis by angleZ degrees"""
    angle = angleZ * math.pi / 180
    return array([[cos(angle), sin(angle), 0],
                  [-sin(angle), cos(angle), 0],
                  [0, 0, 1]])

def getXRotationMatrix(angleX):
    angle = angleX * math.pi / 180
    return array([[1, 0, 0],
                  [0, cos(angle), sin(angle)],
                  [0, -sin(angle), cos(angle)]])

def getYRotationMatrix(angleY):
    angle = angleY * math.pi / 180
    return array([[cos(angle), 0, -sin(angle)],
                  [0, 1, 0],
                  [sin(angle), 0, cos(angle)]])

def getTrMatrix(tr):
    t = eye(4)
    t[:3, 3] = tr
    return t

def getAxisRotationMatrix(axis, angle):
    c = cos(angle)
    s = sin(angle)
    t = 1 - c
    X = axis[0]
    Y = axis[1]
    Z = axis[2]
    matx = array([[t * X * X + c, t * X * Y + s * Z, t * X * Z - s * Y, 0],
                  [t * X * Y - s * Z, t * Y * Y + c, t * Y * Z + s * X, 0],
                  [t * X * Z + s * Y, t * Y * Z - s * X, t * Z * Z + c, 0],
                  [0, 0, 0, 1]])
    return matx

def unitV(vector):
    normVal = linalg.norm(vector)
    if normVal - 1 < 1e-6:
        return vector
    return vector / normVal

def rpyFromRotMatrix(rotMatrix):

    yaw = math.atan2(rotMatrix[2][1], rotMatrix[2][2])   # rotate about x0
    pitch = math.atan2(-rotMatrix[2][0], sqrt(1.00000001 - rotMatrix[2][0] * rotMatrix[2][0])) # rotate about y0
    roll = math.atan2(rotMatrix[1][0], rotMatrix[0][0]) # rotate about z0

    #print "yaw ", yaw * 180/pi
    #print "pitch a ", pitch * 180/pi
    #print "roll a ", roll * 180/pi
    #print
    return array([roll, pitch, yaw])


def rotMatrixFromRpy(rollRad, pitchRad, yawRad):

    cr = cos(rollRad)
    sr = sin(rollRad)
    cp = cos(pitchRad)
    sp = sin(pitchRad)
    cy = cos(yawRad)
    sy = sin(yawRad)

    R00 = cr * cp
    R01 = -sr * cy + cr * sp * sy
    R02 = sr * sy + cr * sp * cy

    R10 = sr * cp
    R11 = cr * cy + sr * sp * sy
    R12 = -cr * sy + sr * sp * cy

    R20 = -sp
    R21 = cp * sy
    R22 = cp * cy

    return array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]])
